/*
 Master
 
 Authors: Matthias Robbers and Niels Winkler
 
 This is the sketch for the Arduino board which holds the 
 two distance sensors, their step motors and magnetical 
 sensors that are used to let the step motors find the right 
 starting point. Two pins are used to communicate with the
 other board.
 */

#include <Stepper.h>
#include <Wire.h>
#include "URMSerial.h"

// Step motor setup
#define STEPS 200
Stepper stepper(STEPS, 8, 9, 6, 7);

// variable for the current angle of the step motors
int view = 0;

// the current direction of the movement of the step motors
const int left = 1;
const int right = 2;
int direction = left;

// counter for steps to reduce the number of distance 
// measurements
int count = 0;

// pins
int magPin1 = A1;
int magPin2 = A0;
int frontPin1 = A2;
int frontPin2 = A3;
int backPin1 = 3;
int backPin2 = 5;
int startDistancePin = 12;

// constants for URM sensor type
#define DISTANCE 1
#define TEMPERATURE 2
#define ERROR 3
#define NOTREADY 4
#define TIMEOUT 5

// distance sensors
URMSerial urm1;
URMSerial urm2;

void setup()
{
  pinMode(startDistancePin, INPUT);
  pinMode(magPin1, INPUT);
  pinMode(magPin2, INPUT);
  pinMode(backPin1, OUTPUT);
  pinMode(backPin2, OUTPUT);
  pinMode(frontPin1, OUTPUT);
  pinMode(frontPin2, OUTPUT);
  pinMode(backPin1, OUTPUT);
  pinMode(backPin2, OUTPUT);

  // start serial monitor
  Serial.begin(9600); 

  // start the distance sensors
  urm1.begin(10,2,9600); // RX Pin, TX Pin, Baud Rate
  urm2.begin(11,4,9600); // RX Pin, TX Pin, Baud Rate

  // set the speed of the motor to 30 RPMs
  stepper.setSpeed(20);

  // find start positions
  enableFrontRotation();
  Serial.println("Searching position (front)...");
  while (digitalRead(magPin1) != 0) {
    rotate(50);
    delay(33);  
  }
  disableFrontRotation();
  Serial.println("Done."); 
  enableBackRotation();
  Serial.println("Searching position (back)...");
  while (digitalRead(magPin2) != 0) {
    rotate(50);
    delay(33);  
  }
  Serial.println("Done."); 
  view = 0;
  enableFrontRotation();

  // join i2c bus (address optional for master)
  Wire.begin();

  // give everything some time to initialize
  delay(100);
}

void loop()
{
  if (digitalRead(startDistancePin) == HIGH) {
    rotate(25);
    count++;
    if (count == 10) { // make two measures every 10 steps
      int distanceF = getMeasurement(urm1, DISTANCE);
      Serial.print("Distance Front: ");
      Serial.println(distanceF);

      int distanceB = getMeasurement(urm2, DISTANCE);
      Serial.print("Distance Back: ");
      Serial.println(distanceB);

      // if there is something in the way: send stop command 
      // to the other board
      if (distanceF > -1 && distanceF < 25) { // front
        Serial.println("Sending...");
        Wire.beginTransmission(4); // transmit to device #4
        if (view > 9) {
          Wire.send("STOP:rf;");
        } 
        else if (view <= 9 && view >= -9) {
          Wire.send("STOP:mf;");
        } 
        else if (view < -9) {
          Wire.send("STOP:lf;");
        }
        Wire.endTransmission();    // stop transmitting
        // stop everything for 3 seconds
        delay(3000);
        // release the block
        Wire.beginTransmission(4); // transmit to device #4
        Wire.send("START;"); 
        Wire.endTransmission();    // stop transmitting
        // give the tank 3 seconds to drive away from the 
        // issue
        delay(3000);
      }
      if (distanceB > -1 && distanceB < 25) { // back
        Serial.println("Sending...");
        Wire.beginTransmission(4); // transmit to device #4
        if (view > 9) {
          Wire.send("STOP:lb;");
        } 
        else if (view <= 9 && view >= -9) {
          Wire.send("STOP:mb;");
        } 
        else if (view < -9) {
          Wire.send("STOP:rb;");
        }
        Wire.endTransmission();    // stop transmitting
        // stop everything for 3 seconds
        delay(3000);
        // release the block
        Wire.beginTransmission(4); // transmit to device #4
        Wire.send("START;"); 
        Wire.endTransmission();    // stop transmitting
        // give the tank 3 seconds to drive away from the 
        // issue
        delay(3000);
      }
      count = 0;
    }
  }
}

// functions to stop and start the rotation of the step motors
void enableFrontRotation() {
  digitalWrite(frontPin1, HIGH);
  digitalWrite(frontPin2, HIGH);
}

void disableFrontRotation() {
  digitalWrite(frontPin1, LOW);
  digitalWrite(frontPin2, LOW);
}

void enableBackRotation() {
  digitalWrite(backPin1, HIGH);
  digitalWrite(backPin2, HIGH);
}

void disableBackRotation() {
  digitalWrite(backPin1, LOW);
  digitalWrite(backPin2, LOW);
}

// function to make a step
void rotate(int angle) {
  if (direction == left) {
    if (view >= angle) {
      direction = right;
    }
  } 
  else {
    if (view <= -angle) {
      direction = left;
    }
  }

  if (direction == left) {
    stepper.step(1);
    view++;
  } 
  else {
    stepper.step(-1); 
    view--;
  }
}

// function to get a measurement value from the sensor
int value; // This value will be populated
int getMeasurement(URMSerial urm, int mode)
{
  // Request a distance reading from the URM37
  switch(urm.requestMeasurementOrTimeout(mode, value)) // Find out the type of request
  {
  case DISTANCE: // Double check the reading we recieve is of DISTANCE type
    return value;
    break;
  case TEMPERATURE:
    return value;
    break;
  case ERROR:
    break;
  case NOTREADY:
    break;
  case TIMEOUT:
    break;
  } 

  return -1;
}